package roborally;

import be.kuleuven.cs.som.annotate.*;

/**
 * A class for dealing with robots.
 * 
 * @invar	The x-coordinate of each robot must be a legal
 *          x-coordinate for a robot.
 *          | isValidX(getX())
 * @invar	The y-coordinate of each robot must be a legal
 *          y-coordinate for a robot.
 *          | isValidY(getY())
 * @invar	The orientation of each robot must be a legal
 *          orientation for a robot.
 *          | isValidOrientation(getOrientation())
 * @invar	The amount of energy of each robot must be a legal
 *         	amount of energy for a robot.
 *          | isValidEnergy(getEnergy())
 * 
 * @version 1.0
 * @author  Pieter Robberechts	(1e Bach Informatica)
 * @author	Jeroen Sanders		(1e Bach Informatica)
 */
public class Robot implements IRobot{
	
	/**
	 * Initialize this new robot with given x-coordinate, y-coordinate, orientation and energy.
	 * 
	 * @param	x
	 * 			x-coordinate of the robot
	 * @param 	y
	 * 			y-coordinate of the robot
	 * @param 	orientation
	 * 			The robot's orientation.
	 * @param	energy
	 * 			The robot's energy
	 * @pre		The given amount of energy must be a valid amount of energy for a robot.
	 * 			| isValidEnergy(energy)
	 * @effect	The given x-coordinate is set as the x-coordinate of this robot.
	 * 			| setX(x)
	 * @effect	The given y-coordinate is set as the y-coordinate of this robot.
	 * 			| setY(y)
	 * @effect	The given orientation is set as the orientation of this robot.
	 * 			| setOrientation(orientation)
	 * @effect	The given energy is set as the energy of this robot.
	 * 			| setEnergy(energy)
	 * @throws	IllegalArgumentException
	 * 			The given x-coordinate is invalid.
	 * 			| ! isValidX(x)
	 * @throws	IllegalArgumentException
	 * 			The given y-coordinate is invalid.
	 * 			| ! isValidY(y)
	 * */
	public Robot(long x, long y, int orientation, double energy) throws IllegalArgumentException{
		assert isValidEnergy(energy); 
		setX(x);
		setY(y);
		setOrientation(orientation);
		setEnergy(energy);
	}

	/**
	 * Set the x-coordinate for this robot to the given coordinate.
	 * 
	 * @param 	x
	 * 		  	The robot's x-coordinate.
	 * @post	The x-coordinate of this robot is equal to the given coordinate.
	 *       	| new.getX() == x
	 * @throws	IllegalArgumentException
	 * 			The given coordinate is invalid.
	 * 			| ! isValidX(x)
	 * 
	 */
	@Model @Raw
	private void setX(long x) throws IllegalArgumentException{
		if(!isValidX(x)) {
			throw new IllegalArgumentException();
		}else{
			this.x = x;	
		}
	}
	
	/**
	 * Return the x-coordinate of this robot.
	 * 
	 */
	@Basic @Raw @Override
	public long getX(){
		return x;
	}
	
	/**
	 * Check whether the given x-coordinate is a valid x-coordinate for a robot.
	 * 
	 * @param 	x
	 *         	The x-coordinate to check.
	 * @return 	True if and only if the given coordinate is a value between 0 and Long.MAX_VALUE.
	 *       	| result == ((x >= 0) && (x <= Long.MAX_VALUE))
	 */
	public static boolean isValidX(long x) {
		return ((x >= 0) && (x <= Long.MAX_VALUE));
	}
	
	/**
	 * Variable registering the x-coordinate of this robot.
	 */
	private long x;
	
	/**
	 * Set the y-coordinate for this robot to the given coordinate.
	 * 
	 * @param 	y
	 * 			The robot's y-coordinate.
	 * @post	The y-coordinate of this robot is equal to the given coordinate.
	 *       	| new.getY() == y
	 * @throws	IllegalArgumentException
	 * 			The given coordinate is invalid.
	 * 			| ! isValidY(y)
	 */
	@Model @Raw
	private void setY(long y) throws IllegalArgumentException{
		if(!isValidY(y)) {
			throw new IllegalArgumentException();
		}else{
			this.y = y;	
		}
	}
	
	/**
	 * Return the y-coordinate of this robot.
	 * 
	 */
	@Basic @Raw @Override
	public long getY(){
		return y;
	}
	
	/**
	 * Check whether the given y-coordinate is a valid y-coordinate for a robot.
	 * 
	 * @param  	y
	 *         	The y-coordinate to check.
	 * @return 	True if and only if the given coordinate is a value between 0 and Long.MAX_VALUE.
	 *       	| result == ((y >= 0) && (y <= Long.MAX_VALUE))
	 */
	public static boolean isValidY(long y) {
		return ((y >= 0) && (y <= Long.MAX_VALUE));
	}
	
	/**
	 * Variable registering the y-coordinate of this robot.
	 */
	private long y;
	
	/**
	 * Set the orientation for this robot to the given orientation.
	 * 
	 * @param 	orientation
	 * 			The robot's orientation. 
	 * @post   	If the given orientation is a valid orientation, 
	 * 			the orientation of this robot is set to the given orientation.
	 *       	| if ( isValidOrientation(orientation) )
	 *       	|   then new.getOrientation() = orientation
	 * @post   	If the given orientation is an invalid orientation,
	 *         	the orientation of this robot remains unchanged.
	 *       	| if ( ! isValidOrientation(orientation) )
	 *       	|   then new.getOrientation() = getOrientation()
	 * @note	0 => up
	 * 			1 => right
	 * 			2 => down
	 * 			3 => left      
	 */
	@Model @Raw
	private void setOrientation(int orientation) {
		if (isValidOrientation(orientation))
			this.orientation = orientation;	
	}
	
	/**
	 * Return the orientation of this robot.
	 * 
	 */
	@Basic @Raw @Override
	public int getOrientation(){
		return orientation;
	}
	
	/**
	 * Check whether the given orientation is a valid orientation for a robot.
	 * 
	 * @param  	orientation
	 *         	The orientation to check.
	 * @return 	True if and only if the given orientation is equal to 0, 1, 2 or 3.
	 *       	| result == (orientation == 0 || orientation == 1 || orientation == 2 || orientation == 3)
	 */
	public static boolean isValidOrientation(int orientation) {
		return (orientation == 0 || orientation == 1 || orientation == 2 || orientation == 3);
	}
	
	/**
	 * Variable registering the orientation of this robot.
	 */
	private int orientation;
	
	/**
	 * Set the amount of energy of this robot to the given amount of energy.
	 * 
	 * @param 	energy
	 * 			The robot's amount of energy.
	 * @pre		The given amount of energy must be a valid amount of energy.
	 * 			| isValidEnergy(energy)
     * @post   	The amount of energy of this robot is equal to the given amount of energy.
     *       	| new.getEnergy() == energy
	 */
	@Model @Raw
	private void setEnergy(double energy) {
		assert isValidEnergy(energy); 
		this.energy = energy;
	}
	
	/**
	 * Return the amount of energy of this robot.
	 * 
	 */
	@Raw @Override
	public double getEnergy(){
		return energy;
	}
	
	/**
	 * Return the maximal amount of energy for a robot.
	 * 
	 */
	@Immutable
	public static double getMaxEnergy(){
		return 20000;
	}
	
	/**
     * Check whether the given amount of energy is a valid amount of energy for any robot.
     * 
     * @param  	energy
     * 		   	The amount of energy to check.
     * @return 	True if and only if the given amount of energy is positive (inclusive zero)
     * 			and less than or equal to the maximal amount of energy for a robot.
     *       	| result == (energy >= 0 && energy <= getMaxEnergy())
	 */
	public static boolean isValidEnergy(double energy) {
		return (energy >= 0 && energy <= getMaxEnergy());
	}
	
	/**
	 * Variable registering the amount of energy of this robot.
	 */
	private double energy;

	/**
	 * Move a robot one position forward.
	 * 
	 * @effect	If the robot is oriented upwards, the y-coordinate of this robot is lowered with one.
	 *       	| if (getOrientation() == 0)
	 * 			|	then setY(getY() - 1)
	 * @effect	If the robot is oriented to the right, the x-coordinate of this robot is raised with one.
	 * 			| if (getOrientation() == 1)
	 * 			|	then setX(getX() + 1)
	 * @effect	If the robot is oriented downwards, the y-coordinate of this robot is raised with one.
	 * 			| if (getOrientation() == 2)
	 * 			|	then setY(getY() + 1) 
	 * @effect	If the robot is oriented to the left, the x-coordinate of this robot is lowered with one.
	 * 			| if (getOrientation() == 3)
	 * 			|	then setX(getX() - 1) 
	 * @effect	The robot's amount of energy is lowered with 500 Ws.
	 * 			| setEnergy(getEnergy() - 500)    
	 * @throws 	IllegalMoveException
     *         	The robot moves to a coordinate which is an invalid coordinate.
     *      	| (getOrientation() == 0 && !isValidY(getY() - 1)) || 
     *      	|	(getOrientation() == 1 && !isValidX(getX() + 1)) || 
     *      	|	(getOrientation() == 2 && !isValidY(getY() + 1)) || 
     *      	|	(getOrientation() == 3 && !isValidX(getX() - 1))
     * @throws	IllegalMoveException
     *        	The robot hasn't got enough energy (less than 500 Ws) to perform this move.
     *         	| getEnergy() < 500  
	 * @note	Moving consumes 500 Ws
	 */
	@Override
	public void move(){
		if (getEnergy() >= 500){
			try {
				if (getOrientation() == 0) setY(getY() -1);
				else if (getOrientation() == 1) setX(getX() + 1);
				else if (getOrientation() == 2) setY(getY() + 1);
				else if (getOrientation() == 3) setX(getX() - 1);
				setEnergy(getEnergy() - 500);
			} catch (IllegalArgumentException e) {
				assert((getOrientation() == 0 && !isValidY(getY() - 1)) || 
					   (getOrientation() == 1 && !isValidX(getX() + 1)) || 
					   (getOrientation() == 2 && !isValidY(getY() + 1)) || 
					   (getOrientation() == 3 && !isValidX(getX() - 1)));
			    throw new IllegalMoveException("Unable to move the robot to this position!");
			}
		}else throw new IllegalMoveException("The robot has insufficient energy to perform this move!");
	}

	/**
	 * Turn a robot 90 degrees in clockwise direction.
	 * 
	 * @effect	If the robot has sufficient energy to rotate, it is rotated 90 degrees clockwise.
	 * 		 	| if(getEnergy() >= 100)
	 *       	|	then setOrientation((getOrientation() + 1)%4)
	 * @effect	The amount of energy of the robot is lowered with 100 Ws if he has enough energy to rotate.
	 * 			| setEnergy(getEnergy() - 100)
	 * @post 	If the robot has insufficient energy, the robots state will remain unchanged.
	 * 		 	| if(getEnergy() < 100)
	 * 		 	|	then (new.getOrientation() == getOrientation()) && (new.getEnergy() == getEnergy())
	 * @note 	Turning consumes 100 Ws.
	 */
	@Override
	public void turnClockwise() {
		if(getEnergy() >= 100) {
			setOrientation((getOrientation() + 1)%4);
			setEnergy(getEnergy() - 100);
		}
	}

	/**
	 * Turn a robot 90 degrees in counter-clockwise direction.
	 * 
	 * @effect	If the robot has sufficient energy to rotate, it is rotated 90 degrees counter-clockwise.
	 * 		 	| if(getEnergy() >= 100)
	 * 			|	if(getOrientation() == 0)
	 *			|		setOrientation(3);
	 *			| 	else
	 *			|		setOrientation((getOrientation() - 1)%4);
	 * @effect	The amount of energy of the robot is lowered with 100 Ws if he has enough energy to rotate.
	 * 			| setEnergy(getEnergy() - 100)
	 * @post 	If the robot has insufficient energy, the robots state will remain unchanged.
	 * 		 	| if(getEnergy() < 100)
	 * 		 	|	then (new.getOrientation() == getOrientation()) && (new.getEnergy() == getEnergy())
	 * 
	 * @note 	Turning consumes 100 Ws.
	 */
	public void turnCounterClockwise() {
		if(getEnergy() >= 100) {
			if(getOrientation() == 0)
				setOrientation(3);
			else
				setOrientation((getOrientation() - 1)%4);
			setEnergy(getEnergy() - 100);
		}
	}
	
	/**
	 * Recharges the robot with a given amount of energy.
	 * 
	 * @param 	energyAmount
	 * 		  	| The amount of energy to add.
	 * @pre		The given amount of energy to be recharged must be positive (inclusive zero).
	 * 			| energyAmount >= 0
	 * @pre 	The given amount of energy to be recharged in combination with the actual amount of energy of the robot 
	 * 			must be a valid amount energy for a robot.
	 * 			| isValidEnergy(getEnergy() + energyAmount)
	 * @effect	The robots energy will be increased by the given amount of energy.
	 * 			| setEnergy(getEnergy() + energyAmount)
	 */
	@Override
	public void recharge(double energyAmount) {
		assert energyAmount >= 0;
		assert isValidEnergy(getEnergy() + energyAmount);
		setEnergy(getEnergy() + energyAmount);
	}
	
	/**
	 * Returns the relative amount of energy as a double.
	 * 
	 */
	public double getEnergyPercentage() {
		return (getEnergy()/getMaxEnergy());
	}
	
	/**
	 * Return the minimal amount of energy to reach a particular position.
	 * 
	 * @param	x
	 * 			The x-coordinate of the destination for the given robot.
	 * @param	y
	 * 			The y-coordinate of the destination for the given robot.
	 * @pre		The given x-coordinate must be a valid x-coordinate for a robot.
	 * 			| isValidX(x)
	 * @pre		The given y-coordinate must be a valid y-coordinate for a robot.
	 * 			| isValidY(y)
	 * @return	The amount of consumed energy is the sum of the energy consumed when moving and energy consumed when turning.
	 * 			| result == getManhattanDistance(getX(), getY(), x, y)*500 + countTurnsToReach(x, y)*100
	 * 
	 */
	@Override
	public double getEnergyRequiredToReach(long x, long y) {
		assert(isValidX(x));
		assert(isValidY(y));
		
		//If the robot is located on the given position, he won't consume any energy
		if(x == getX() && y == getY())
			return 0;
		
		//Count the number of turns needed to reach the destination
		int turns = countTurnsToReach(x, y);
				
		//Count the number of moves needed to reach the destination (Manhattan distance)
		long distance = getManhattanDistance(getX(), getY(), x, y);
		
		//Energy consumed when moving + energy consumed when turning
		return distance*500 + turns*100;
	}

	/**
	 * Move two robots as close as possible to each other.
	 * 	If both robots don't have enough energy to move next to each other,
	 * 	move them as close as possible.
	 * 
	 * @param 	robot2
	 * 			The second robot which is involved in the movement.
	 * @post	Both robots end up as close as possible to each other (in optimal case next to each other), 
	 * 			taking into account their initial amounts of energy and with a minimal amount of consumed energy.
	 * 			| for each X1, Y1, X2, Y2:
	 * 			|	if	( isValidX(X1) && isValidY(Y1) && isValidX(X2) && isValidY(Y2) &&
	 * 			|		  this.getEnergy() >= this.getEnergyRequiredToReach(X1, Y1) && 
	 * 			|		  robot2.getEnergy() >= robot2.getEnergyRequiredToReach(X2, Y2) 
	 * 			|	   	)
	 * 			|		then( 	(getManhattanDistance(X1, Y1, X2, Y2) > getManhattanDistance(new.this.getX(), new.this.getY(), new.robot2.getX(), new.robot2.getY())) ||
	 * 			|			 	(getManhattanDistance(X1, Y1, X2, Y2) == getManhattanDistance(new.this.getX(), new.this.getY(), new.robot2.getX(), new.robot2.getY()) &&
	 * 			|				this.getEnergyRequiredToReach(X1, Y1) + robot2.getEnergyRequiredToReach(X2, Y2)	>= 
	 * 			|					this.getEnergyRequiredToReach(new.getX(), new.getY()) + robot2.getEnergyRequiredToReach(new.getX(), new.getY()) ) 
	 * 			|			)
	 * @post	Both robots end up in the direction of their last move.
	 * 			| new.getOrientation() == (getOrientation() + countTurnsToReach(new.getX(), new.getY()))%4
	 * 			| new.robot2.getOrientation() == (robot2.getOrientation() + robot2.countTurnsToReach(new.robot2.getX(), new.robot2.getY()))%4
	 * @post	Both robots lose the amount of energy needed to reach their new destinations.
	 * 			| new.this.getEnergy() == this.getEnergy() - this.getEnergyRequiredToReach(new.this.getX(), new.this.getY()) 
	 * 			| new.robot2.getEnergy() == robot2.getEnergy() - robot2.getEnergyRequiredToReach(new.robot2.getX(), new.robot2.getY())
	 * @throws	IllegalArgumentException
	 * 			The second robot is not effective.
	 * 			| robot2 == null
	 */
	@Override
	public void moveNextTo(IRobot robot2) throws IllegalArgumentException {
		//The second robot should be an effective robot to avoid NullPointerExceptions
		if(robot2 == null)
			throw new IllegalArgumentException("Non-effective robot!");
 
		//Because the fist robot is likely to consume more energy, switch them when the second robot initially has a larger amount of energy
		if(getEnergy() < robot2.getEnergy())
			robot2.moveNextTo(this);
		
		//If both robots initially occupy the same position, the one with the most energy should move
		if(getX() == robot2.getX() && getY() == robot2.getY()){
			try{
				move();
			}catch(IllegalMoveException e){
				if(getEnergy() >= countTurnsToReach(1,1)*100 + 500){
					turnTo(1,1);
					move();
				}
			}
		}
		
		//Both robots are pointing to each other and can start moving without turning
		while( (this.getEnergy() >= 500 || robot2.getEnergy() >= 500) && 
				(isCloserWhenMoved(robot2) || ((Robot) robot2).isCloserWhenMoved(this)) && 
				!isNextTo(robot2) && 
				(countTurnsToReach(robot2.getX(), robot2.getY()) == ((Robot) robot2).countTurnsToReach(getX(), getY()))){
			
			//Move both robots alternating closer without turning
			moveCloserTo(robot2);
			
			if( !isNextTo(robot2) )
				((Robot) robot2).moveCloserTo(this);
		}
		
		//If turning is necessary, calculate the optimal turn-pattern
		if((this.getEnergy() >= 600 || robot2.getEnergy() >= 600) && !isNextTo(robot2)){
			// If it is more effective to turn robot2 twice instead of robot1 once, because robot1 hasn't got enough energy
			if( (getX() == robot2.getX() || getY() == robot2.getY()) && countTurnsToReach(robot2.getX(), robot2.getY()) > 0 && !canGetNextTo(robot2) && ((Robot) robot2).canGetNextTo(this)){
				//Move the second robot to the first
				while(!isNextTo(robot2)){
					((Robot) robot2).moveCloserTo(this);
				}
			// If it is more effective to turn robot1 twice instead of robot2 once, because robot2 hasn't got enough energy	
			}else if((getX() == robot2.getX() || getY() == robot2.getY()) && ((Robot) robot2).countTurnsToReach(getX(), getY()) > 0 && !((Robot) robot2).canGetNextTo(this) && canGetNextTo(robot2)){
				//Move the first robot to the second
				while(!isNextTo(robot2)){
					moveCloserTo(robot2);
				}
			// If robot1 needs less turns than robot2	
			}else if(countTurnsToReach(robot2.getX(), robot2.getY()) <= ((Robot) robot2).countTurnsToReach(getX(), getY())){
				//Try moving the first robot to the second without having to turn the second
				while(getEnergy() >= countTurnsToReach(robot2.getX(), robot2.getY())*100 + 500 && !isNextTo(robot2)){
					moveCloserTo(robot2);
				}
				while(!isNextTo(robot2) && robot2.getEnergy() >= ((Robot) robot2).countTurnsToReach(getX(), getY())*100 + 500){
					((Robot) robot2).moveCloserTo(this);
				}
			// If robot2 needs less turns than robot1	
			} else {
				//Try moving the second robot to the first robot without having to turn the first
				while(robot2.getEnergy() >= ((Robot) robot2).countTurnsToReach(getX(), getY())*100 + 500 && !isNextTo(robot2)){
					((Robot) robot2).moveCloserTo(this);
				}
				while(!isNextTo(robot2) && getEnergy() >= countTurnsToReach(robot2.getX(), robot2.getY())*100 + 500 && !isNextTo(robot2)){
					moveCloserTo(robot2);
				}
			}
		}
		
	}
	
	/**
	 * Check whether a robot has got enough energy to get next to a second robot.
	 */
	private boolean canGetNextTo(IRobot robot2){
		return( getEnergy() >= getEnergyRequiredToReach(robot2.getX()+1, robot2.getY()) ||
				getEnergy() >= getEnergyRequiredToReach(robot2.getX()-1, robot2.getY()) ||
				getEnergy() >= getEnergyRequiredToReach(robot2.getX(), robot2.getY()+1) ||
				getEnergy() >= getEnergyRequiredToReach(robot2.getX(), robot2.getY()-1) );
	}

	/**
	 * Move a robot one step closer to a second robot.
	 * Move the robot one position forward when he comes closer by moving, else turn the robot in the direction of the second robot.
	 * 
	 */
	private void moveCloserTo(IRobot robot2) {
		if(this.getEnergy() >= 500 && isCloserWhenMoved(robot2))
			this.move();
		else if(this.getEnergy() >= countTurnsToReach(robot2.getX(), robot2.getY())*100 + 500)
			turnTo(robot2.getX(), robot2.getY());
	}
	
	/**
	 * Check whether a robot is located next to another robot.
	 * 
	 */
	private boolean isNextTo(IRobot robot2) {
		return 	(getX() == robot2.getX()+1 && getY() == robot2.getY()) ||
				(getX()+1 == robot2.getX() && getY() == robot2.getY()) ||
				(getX() == robot2.getX() && getY() == robot2.getY()+1) ||
				(getX() == robot2.getX() && getY()+1 == robot2.getY());
	}
	
	/**
	 * Check whether a robot will be closer (in manhattan distance) to a second robot
	 * when the move-operation would be executed on it.
	 * 
	 */
	private boolean isCloserWhenMoved(IRobot robot2){
		long distanceBeforeMove = getManhattanDistance(getX(), getY(), robot2.getX(),robot2.getY());
		long distanceAfterMove = distanceBeforeMove;
		if(getOrientation() == 0)
			distanceAfterMove = getManhattanDistance(getX(), getY(), robot2.getX(),robot2.getY()+1);
		if(getOrientation() == 1)
			distanceAfterMove = getManhattanDistance(getX(), getY(), robot2.getX()-1,robot2.getY());
		if(getOrientation() == 2)
			distanceAfterMove = getManhattanDistance(getX(), getY(), robot2.getX(),robot2.getY()-1);
		if(getOrientation() == 3)
			distanceAfterMove = getManhattanDistance(getX(), getY(), robot2.getX()+1,robot2.getY());
		
		return distanceAfterMove < distanceBeforeMove;
	}
	
	/**
	 * Turn a robot 90 degrees in the direction of the position indicated by the the given x-and y-coordinate.
	 * 
	 */
	private void turnTo(long x, long y){
		switch(getOrientation()){
			case 0:
				if(getX() > x)
					turnCounterClockwise();
				else if(getX() == x && getY() < y){
					turnClockwise();
					turnClockwise();
				} else if(getX() < x)
					turnClockwise();
				break;
			case 1:
				if(getY() > y)
					turnCounterClockwise();
				else if(getY() == y && getX() > x){
					turnClockwise();
					turnClockwise();
				} else if(getY() < y)	
					turnClockwise();
				break;
			case 2:
				if(getX() > x)
					turnClockwise();
				else if(getX() == x && getY() > y){
					turnClockwise();
					turnClockwise();
				} else if(getX() < x)
					turnCounterClockwise();
				break;
			case 3:
				if(getY() > y)
					turnClockwise();
				else if(getY() == y && getX() < x){
					turnClockwise();
					turnClockwise();
				} else if( getY() < y)	
					turnCounterClockwise();
				break;
		}
	}
	
	/**
	 * Calculate the amount of turns a robot should make to reach the given destination.
	 *
	 * @param 	x
	 * 			The x-coordinate of the destination.
	 * @param 	y
	 * 			The y-coordinate of the destination.
	 * @return	The amount of turns a robot should make is determined by the robots orientation.
	 * 			| if ((orientation == 0 && x == getX() && y > getY()) ||
	 *			|	 (orientation == 1 && x < getX() && y == getY()) ||
	 *			|	 (orientation == 2 && x == getX() && y < getY()) ||
	 *			|	 (orientation == 3 && x > getX() && y == getY()))
	 *			|		then if (x != getX() && y != getY())
	 *			|				then result == 3
	 *			|			 else
	 *			|				then result == 2
	 *			|else if ((orientation == 0 && y >= getY()) ||
	 *			|		 (orientation == 1 && x <= getX()) ||
	 *			|		 (orientation == 2 && y <= getY()) ||
	 *			|		 (orientation == 3 && x >= getX()))
	 *			|			then if (x != getX() && y != getY())
	 *			|					then result == 2
	 *			|				 else
	 *			|					then result == 1
	 *			|else then result == 0
	 *
	 */
	@Model
	private int countTurnsToReach(long x, long y){
		int turns = 0;
		if ((orientation == 0 && x == getX() && y > getY()) ||
			(orientation == 1 && x < getX() && y == getY()) ||
			(orientation == 2 && x == getX() && y < getY()) ||
			(orientation == 3 && x > getX() && y == getY()))
			turns += 2;
		else if ((orientation == 0 && y >= getY()) ||
				(orientation == 1 && x <= getX()) ||
				(orientation == 2 && y <= getY()) ||
				(orientation == 3 && x >= getX()))
			turns++;

		//One additional turn is necessary if robot and destination aren't located on a horizontal or vertical line
		if(x != getX() && y != getY())
			turns++;
		
		return turns;
	}
	
	/**
	 * Calculate the manhattan distance between 2 positions.
	 * 
	 * @param	x1
	 * 			The x-coordinate of the first position.
	 * @param 	y1
	 * 			The y-coordinate of the first position.
	 * @param	x2
	 * 			The x-coordinate of the second position.
	 * @param 	y2
	 * 			The y-coordinate of the second position.
	 * @return	Return the manhattan distance between the given coordinates.
	 * 			| result == Math.abs(x1-x2) + Math.abs(y1-y2)
	 * 
	 */
	@Model
	private static long getManhattanDistance(long x1, long y1, long x2, long y2){
		return Math.abs(x1-x2) + Math.abs(y1-y2);
	}
	
}
